/*
 * Copyright (c) 2012 Johann Hanne
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the "Software"), to deal
 * in the Software without restriction, including without limitation the rights
 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included in
 * all copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
 * THE SOFTWARE.
 */

#include <stdio.h>
#include <stdint.h>
#include <putchar.h>
#include <string.h>

#include <ethernet.h>
#include <delay.h>
#include <i2c.h>
#include <frmgrb.h>
#include <sensor.h>

#define FRMGRB_X1 2
#define FRMGRB_Y1 1
#define FRMGRB_X2 642
#define FRMGRB_Y2 481

union {
    uint32_t ethernet_alignment_enforcement;
    uint8_t data[1514];
} ethernet_ip_udp_frame = { .data = {
    /* Destination ethernet address - multicast mapping for 239.192.1.1 */
    0x01, 0x00, 0x5e, 0x40, 0x01, 0x01,
    /* Source ethernet address */
    0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
    /* ethertype IPv4 */
    0x08, 0x00,
    /* IP version 4, header length */
    0x45,
    /* DSCP/ECN */
    0x00,
    /* Total length (IP header + UDP header + payload */
    0x00, 0x00, /* must be changed dynamically */
    /* Identification */
    0x12, 0x34,
    /* Flags (Don't fragment), fragment offset */
    0x40, 0x00,
    /* TTL */
    0x40,
    /* Protocol: UDP */
    0x11,
    /* Header checksum */
    0x00, 0x00, /* must be changed dynamically */
    /* Source */
    192, 168, 9, 1,
    /* Destination */
    239, 192, 1, 1,
    /* Source port */
    0xff, 0xff,
    /* Destination port 1234 */
    0x04, 0xd2,
    /* Length (UDP header + payload) */
    0x00, 0x00, /* must be changed dynamically */
    /* Checksum (0x0000 means "checksum unused") */
    0x00, 0x00,
}};

static void set_udp_ip_length(uint8_t *packet, uint16_t payload_length) {
    uint16_t total_length = payload_length + 28;
    uint16_t udp_length = payload_length + 8;
    uint16_t ip_chksum = 0xffff;
    uint8_t *p;
    uint16_t w;

    packet[16] = (total_length & 0xff00) >> 8;
    packet[17] =  total_length & 0x00ff;

    packet[38] = (udp_length & 0xff00) >> 8;
    packet[39] =  udp_length & 0x00ff;

    packet[24] = 0x00;
    packet[25] = 0x00;

    for (p = packet + 14;
         p < packet + 34;
         p += 2) {
        w = ((uint16_t)(*p) << 8) | (uint16_t)(*(p + 1));
        if (w > ip_chksum) {
            w++;
        }
        ip_chksum -= w;
    }

    packet[24] = (ip_chksum & 0xff00) >> 8;
    packet[25] =  ip_chksum & 0x00ff;
}

static uint8_t *segbases[6] = {
    (uint8_t *)0x06000000ul,
    (uint8_t *)0x06c80000ul,
    (uint8_t *)0x07900000ul,
    (uint8_t *)0x08580000ul,
    (uint8_t *)0x09200000ul,
    (uint8_t *)0x09e80000ul
};

void main(void) {
    uint8_t i2c_addr;
    uint16_t width, height;
    int seg;
    uint8_t *addr;
    int chunk;
    uint8_t sizeinfo[4];

    ethernet_init();
    ethernet_get_mac_address(ethernet_ip_udp_frame.data + 6);

    printf("Value of port 0x826 at boot: 0x%04x\n", inpw(0x826));
    /* 0x826 seems to contain some kind of clock divider; not setting it to 1
       reduces the frame capture rate and maybe also the performance of other
       peripherals */
    outpw(0x826, 0x0001);

    printf("Initializing image sensor...\n");
    sensor_init();
    /* Basically, the sensor also works with its reset defaults; however,
       the SQ610 frame grabber needs a slightly increased window size to
       work correctly; with the sensor default window size, the grab rate
       will be effectively halved */
    sensor_set_defaults();

    frmgrb_init();

    frmgrb_set_rectangle(FRMGRB_X1, FRMGRB_Y1, FRMGRB_X2, FRMGRB_Y2);
    width = FRMGRB_X2 - FRMGRB_X1;
    height = FRMGRB_Y2 - FRMGRB_Y1;

    frmgrb_set_destination_address(segbases[0]);

    frmgrb_control(0, 0);

    for (;;) {

        frmgrb_control(3, 1);

        while ((frmgrb_status() & 0x01) == 0)
            ;

        frmgrb_control(0, 0);

        /*
         * 640*480 = 307200
         *
         * 307200/1280 = 240
         * so we send 240 UDP/IP packets with 1280 bytes UDP payload each
         */
        set_udp_ip_length(ethernet_ip_udp_frame.data, 1280);
        /*
         * 307200/6 = 51200 = 0xc800
         * so we use 6 segments with 51200 bytes each to access all the data
         * written by the frame grabber
         */
        for (seg = 0; seg < 6; seg++) {
            addr = segbases[seg];
            /*
             * 240/6 = 40
             * so we use 40 chunks per segment
             */
            for (chunk = 0; chunk < 40; chunk++) {
                ethernet_tx_direct(ethernet_ip_udp_frame.data, 14 + 20 + 8, addr, 1280);
                addr += 1280;
            }
        }

        /* Transmit size information */
        set_udp_ip_length(ethernet_ip_udp_frame.data, 4);
        sizeinfo[0] =  (width & 0xff00) >> 8;
        sizeinfo[1] =   width & 0x00ff;
        sizeinfo[2] = (height & 0xff00) >> 8;
        sizeinfo[3] =  height & 0x00ff;
        ethernet_tx_direct(ethernet_ip_udp_frame.data, 14 + 20 + 8, sizeinfo, 4);

        printf(".");
    }
}
